#ifndef __MPU6050_H__
#define __MPU6050_H__

#include "main.h"
#include <stdint.h>

#ifdef __cplusplus
extern "C" {
#endif

/* ==== 常量/枚举 ==== */
#define MPU6050_ADDR_AD0_LOW   (0x68)  // AD0 接地
#define MPU6050_ADDR_AD0_HIGH  (0x69)  // AD0 拉高

typedef enum {
  MPU6050_OK = 0,
  MPU6050_ERR
} MPU6050_Result;

typedef enum {
  MPU6050_ACC_2G = 0,
  MPU6050_ACC_4G,
  MPU6050_ACC_8G,
  MPU6050_ACC_16G
} MPU6050_AccelRange;

typedef enum {
  MPU6050_GYRO_250DPS = 0,
  MPU6050_GYRO_500DPS,
  MPU6050_GYRO_1000DPS,
  MPU6050_GYRO_2000DPS
} MPU6050_GyroRange;

typedef enum {
  MPU6050_DLPF_260HZ = 0, // ~260Hz(acc)/256Hz(gyro)
  MPU6050_DLPF_184HZ = 1,
  MPU6050_DLPF_94HZ  = 2,
  MPU6050_DLPF_44HZ  = 3,
  MPU6050_DLPF_21HZ  = 4,
  MPU6050_DLPF_10HZ  = 5,
  MPU6050_DLPF_5HZ   = 6
} MPU6050_DLPF;

/* ==== 设备对象 ==== */
typedef struct {
  I2C_HandleTypeDef *hi2c;  // I2C 句柄
  uint8_t addr7;            // 7位地址：0x68 或 0x69

  /* 量程/灵敏度 */
  MPU6050_AccelRange acc_range;
  MPU6050_GyroRange  gyro_range;
  float acc_sens;   // LSB->g
  float gyro_sens;  // LSB->deg/s

  /* 标定偏置（工程单位） */
  float gyro_bias[3];   // °/s
  float acc_bias[3];    // g（一般保持 0 即可）

  /* 互补滤波 */
  float alpha;          // 0.90~0.999，越大越信陀螺
  float pitch_deg;      // 俯仰（X 前后）
  float roll_deg;       // 横滚（Y 左右）

  /* 计时（用于 dt） */
  uint32_t last_us;
  uint8_t  use_dwt_time; // 1=用DWT计时 0=用HAL_Tick近似
} MPU6050_t;

/* ==== 原始数据结构（单位：原始adc）==== */
typedef struct {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  int16_t temp;
} MPU6050_Raw;

/* ==== API ==== */
MPU6050_Result MPU6050_Init(MPU6050_t *dev,
                            I2C_HandleTypeDef *hi2c,
                            uint8_t addr7,
                            MPU6050_AccelRange acc_range,
                            MPU6050_GyroRange gyro_range,
                            MPU6050_DLPF dlpf,
                            uint8_t smplrt_div,   // 采样分频：采样率 = 1k/(1+div)
                            float alpha,          // 互补滤波参数
                            uint8_t use_dwt_time);

MPU6050_Result MPU6050_ReadRaw(MPU6050_t *dev, MPU6050_Raw *out);
float          MPU6050_ReadTempC(MPU6050_t *dev);  // 摄氏温度
MPU6050_Result MPU6050_SetRanges(MPU6050_t *dev,
                                 MPU6050_AccelRange acc_range,
                                 MPU6050_GyroRange gyro_range);
MPU6050_Result MPU6050_SetDlpfSmpl(MPU6050_t *dev, MPU6050_DLPF dlpf, uint8_t smplrt_div);

/* 简单陀螺零偏标定（静置 N 次平均） */
MPU6050_Result MPU6050_CalibrateGyro(MPU6050_t *dev, uint16_t samples, uint16_t delay_ms);

/* 互补滤波更新俯仰/横滚（自动计算 dt） */
MPU6050_Result MPU6050_UpdateAngles(MPU6050_t *dev);

/* 工具：把原始值换算成工程单位 */
static inline void MPU6050_ConvertAccel(const MPU6050_t *dev, const MPU6050_Raw *r, float g_xyz[3]) {
  g_xyz[0] = (float)r->ax / dev->acc_sens - dev->acc_bias[0];
  g_xyz[1] = (float)r->ay / dev->acc_sens - dev->acc_bias[1];
  g_xyz[2] = (float)r->az / dev->acc_sens - dev->acc_bias[2];
}
static inline void MPU6050_ConvertGyro(const MPU6050_t *dev, const MPU6050_Raw *r, float dps_xyz[3]) {
  dps_xyz[0] = (float)r->gx / dev->gyro_sens - dev->gyro_bias[0];
  dps_xyz[1] = (float)r->gy / dev->gyro_sens - dev->gyro_bias[1];
  dps_xyz[2] = (float)r->gz / dev->gyro_sens - dev->gyro_bias[2];
}

#ifdef __cplusplus
}
#endif
#endif /* __MPU6050_H__ */
